#include <stdlib.h>
#include <time.h>
#include <stdio.h>

#define _MODELO_CPP_

#include "modelo.h"



CModel2D::CModel2D(CModel3D *model3d, HaarDetector *haars, int n) : ahaars(haars), nhaars(n)
{
    pmodel3D = model3d;
    points2D =0;
    mask_hide =0;
    inivect =0;
    vect = translation =0;
    vect2 = translation2 =0;
    vect_media = translation_media = 0;
    rotation_tmp =0;
    frame =0;
    pmodel3D =0;
    epilines =0;
}

//CModel2D::CModel2D(void)
//{
//    ;
//}

CModel2D:: ~CModel2D()
{
    if (epilines) cvReleaseMat(&epilines);
    if (points2D) cvReleaseMat(&points2D);
    if (mask_hide) cvReleaseMat(&mask_hide);
    if (inivect) cvReleaseMat(&inivect);
    if (vect) cvReleaseMat(&vect);
    if (vect_media) cvReleaseMat(&vect_media);
    if (vect2) cvReleaseMat(&vect2);
    if (translation_media) cvReleaseMat(&translation_media);
    if (translation2) cvReleaseMat(&translation2);
    if (translation) cvReleaseMat(&translation);
    if (rotation_tmp) cvReleaseMat(&rotation_tmp);
    if (frame) cvReleaseImage(&frame);
    
};


int CModel2D::detect (int n, IplImage * img)
{
    haar_rect = ahaars[n].detect(img, HAAR_MIN_SIZE, false);
    if (haar_rect.x==0 && haar_rect.y == 0)
	return 0; //no se encontró la cara;
    return 1;
}
CvRect CModel2D::set_reduced_haar (void)
{
    float quitar_x = 1/6.0f;
    float quitar_y = 0;////1/8.0f;
    haar_rect2 = cvRect(haar_rect.x + (int)(haar_rect.width * quitar_x), 
			haar_rect.y + (int)(haar_rect.height * quitar_y), 
			(int)(haar_rect.width* (1-2*quitar_x)), 
			(int)(haar_rect.height* (1-2*quitar_y)));

    return haar_rect2;
}
CvRect CModel2D::get_haar(void)
{
    return haar_rect2;
}
void CModel2D::set_haar(CvRect rect2)
{
    haar_rect2 = rect2;
}

CvMat * CModel2D::copy_data(CvPoint2D32f * points2D32f, int npoints)
{
    CvMat pointsI = cvMat(npoints, 2, CV_32FC1, points2D32f);
    if (points2D)
	cvReleaseMat(&points2D);
    points2D = cvCloneMat(&pointsI);
    return points2D;
}
CvMat * CModel2D::set_data(CvPoint2D32f * points2D32f, int npoints)
{
    if (points2D)
	cvReleaseMat(&points2D);
    points2D = cvCreateMatHeader(npoints, 2, CV_32FC1);
    cvSetData(points2D, points2D32f, CV_AUTOSTEP);
    return points2D;
}


void CModel3D::setpoints(int row, CvMat *XYZi)
{
    cvmSet(points3D, row, 0, cvmGet(XYZi, 0, 0));
    cvmSet(points3D, row, 1, cvmGet(XYZi, 1, 0));
    cvmSet(points3D, row, 2, cvmGet(XYZi, 2, 0));
}

void CModel2D::setpoints(int row, CvMat *XYZi)
{
    cvmSet(points2D, row, 0, cvmGet(XYZi, 0, 0));
    cvmSet(points2D, row, 1, cvmGet(XYZi, 1, 0));
}
void CModel2D::setpoints(int row, float ur, float vr)
{
    cvmSet(points2D, row, 0, ur);
    cvmSet(points2D, row, 1, vr);
}

int CModel2D::clear_mat(CvMat * maskin)
{
    int npoints = cvCountNonZero(maskin);

    return npoints;
}
int CModel3D::clear_mat(CvMat * maskin)
{
    int npoints = cvCountNonZero(maskin);

    return npoints;
}


int CModel3D::do_Estereo (CModel2D *model2D, IplImage ** image, int ncams, 
			  struct corners_params_t *cp, bool graficar)
{   
    IplImage *grey[2];
    int i;
    HaarDetector haar;

    float a=0, b=0, absb=0, g=0;
    int nhaar =0;
    a= cvmGet(model2D[0].inivect, 0, 0);
    b= cvmGet(model2D[0].inivect, 1, 0);
    g= cvmGet(model2D[0].inivect, 2, 0);
    printf("Vector inicial:  %f,%f,%f\n", a, b, g);
	

    //aplicación del haar-like para detectar la posición de la cara en las vistas
    if (b<0) absb= -b; else absb=b;

    if (absb > 5 && absb<=25)
    {
	nhaar = 0;
    }
    else if (absb>25)
    {
	nhaar = 1;
    }
    else
    {
	nhaar = 0;
    }


    haar.set_debug(false);

    for (i=0; i<ncams; i++)
    {
	grey[i] = cvCreateImage( cvGetSize(image[i]), 8, 1 );
	if(image[i]->nChannels!=1)
	    cvCvtColor( image[i], grey[i], CV_BGR2GRAY );
	else
	    cvCopy(image[i], grey[i]);

	if (!model2D[i].detect(nhaar, grey[i]))
	{
	    //no se encontró la cara. Se pasa el frame
	    cvReleaseImage(&grey[0]);
	    if (grey[1]) cvReleaseImage(&grey[1]);

	    return 0;
	}
	
	CvRect haar_rect2 = model2D[i].set_reduced_haar();

	if (absb > 5 && absb<=25)
	{
	    haar_rect2.x +=(int)(3*b);
	    haar_rect2.width +=(int)(1*b);
	    model2D[i].set_haar(haar_rect2);
	}
    }    


    npoints = 35;
    model2D[0].points2D = cvCreateMat(npoints, 2, CV_32FC1);
    model2D[1].points2D = cvCreateMat(npoints, 2, CV_32FC1);
    CvPoint2D32f * points2D32f = (CvPoint2D32f *) model2D[0].points2D->data.fl;
    
    cvSetImageROI(grey[0], model2D[0].get_haar());
    npoints = FindTrackPoints( grey[0], points2D32f, npoints, cp);
    cvResetImageROI(grey[0]);
    
    printf("goodfeaturestotrack %d\n", npoints);
    fflush(stdout);

    model2D[0].FindStereoPoints(grey, &model2D[1], graficar);

    //busca los puntos que se salen del modelo
    int ipoints = cvCountNonZero(mask);
    printf("Puntos del modelo: %d.\n", ipoints);
    fflush(stdout);
    if (ipoints > 20)
    {
	RANSAC_circulo(20.0F, N_ITERATIONS_RANSAC_FIT_CIRCLE,  points3D, mask);
	ipoints = cvCountNonZero(mask);
    }
    else
	exit(-1);

    //cambia el tamaño de las matrices, para eliminar los puntos descartados
    ipoints = clear_mat(mask);
    model2D[0].clear_mat(mask);
    model2D[0].clear_mat(mask);
    
    printf("Puntos del modelo: %d.\n", ipoints);

    angY = cvCreateMat(ipoints, 2, CV_32FC1);
    //     cvSet(*angY, cvScalar(70.0*CV_PI/180.0));
    //     float tdata[] = {1.0F, 0.0F, 0.0F, -1.0F};
    //     CvMat tdatamat = cvMat(2, 2, CV_32FC1, tdata);
    //     cvMatMul(*angY, &tdatamat, *angY);
    GetAngles(points3D, angY);

    cvReleaseImage(&grey[0]);
    cvReleaseImage(&grey[1]);
    cvReleaseMat(&mask);

    return ipoints;
}


void CModel2D::ComputeCorrespondEpilines(void)
{
    if (!epilines)
	epilines = cvCreateMat(points2D->rows, 3, CV_32FC1);
    cvComputeCorrespondEpilines(points2D, 1, cal->Fundamental, epilines);
}


int CModel2D::FindStereoPoints(IplImage ** grey, CModel2D *model2D2, bool graficar)
			       
{
    int npoints = points2D->rows;
    int i, method = 5;
    IplImage * n_mask;
    int methods[6] = { CV_TM_SQDIFF, CV_TM_SQDIFF_NORMED, CV_TM_CCORR, 
		       CV_TM_CCORR_NORMED, CV_TM_CCOEFF, CV_TM_CCOEFF_NORMED};
    CvPoint minpos, maxpos, maxpos2;
    double min, max;
    IplImage *result;
    CvMat *XYZi = cvCreateMat(3,1,CV_32F);

    float ur, vr, pt1yfloat;
    CvPoint pt0, pt1, pt2;

    CvRect roiresult, roimatch, roipatch, roigraph;
    CvSize match2[] = {{50, 100}, {30, 30}, {10, 10}};
    CvSize match;

    CvMat * graph1d, *suma1d;
    IplImage * graph = NULL;
    int w = model2D2->get_haar().width; //cvGetSize(grey[0]).width;
    int h = 100;
    IplImage *grey2[2];

    graph1d = cvCreateMat(1, w, CV_32FC1);
    suma1d = cvCreateMat(1, w, CV_32FC1);

    if (graficar)
    {
	grey2[0] = cvCloneImage(grey[0]);
	grey2[1] =cvCloneImage(grey[1]);
	
	graph = cvCreateImage(cvSize(cvGetSize(grey[0]).width, h*2+10), IPL_DEPTH_32F, 3);
	cvZero(graph);

	roigraph.y=0;
	roigraph.height = cvGetSize(grey[0]).height;
	roigraph.x = model2D2->get_haar().x;
	roigraph.width = model2D2->get_haar().width;
	cvSetImageROI(graph, roigraph);
	cvNamedWindow("graph");
    }
    
    result = cvCreateImage(cvGetSize(grey[1]), IPL_DEPTH_32F, 1);
    n_mask = cvCreateImage(cvGetSize(grey[1]), IPL_DEPTH_8U, 1);
    cvZero(result);

    pmodel3D->mask = cvCreateMat(npoints, 1, CV_8UC1);

    ComputeCorrespondEpilines();
    
    cvResetImageROI(grey[0]);
    cvResetImageROI(grey[1]);
    
    //se establece la zona de búsqueda dentro del aḿbito de la cara, según la ROI del harr-like
    roiresult.width = model2D2->get_haar().width;
    roiresult.x = model2D2->get_haar().x;

    cvZero(pmodel3D->mask);

    //para debug
    bool mostrar_epi_lineas_poco_a_poco=false;
    if (0)
    {
	cvNamedWindow("h1");
	cvNamedWindow("h2");
	mostrar_epi_lineas_poco_a_poco=true;
    }
    //////

    for (i=0; i<npoints; i++)
    {
	//array acumulador de los resultados de las distintas correlaciones
	cvZero(suma1d);
	
	pt0.x = (int)cvmGet(points2D, i, 0);
	pt0.y = (int)cvmGet(points2D, i, 1);

	//linea para la máscara
	pt1.x = model2D2->get_haar().x;
	pt2.x = model2D2->get_haar().x + model2D2->get_haar().width;

	pt1yfloat = -(float)cvmGet(epilines, i, 2)/(float)cvmGet(epilines, i, 1);
	pt1.y = (int)(pt1yfloat - (float)cvmGet(epilines, i, 0)*pt1.x/(float)cvmGet(epilines, i, 1));
	pt2.y = (int)(pt1yfloat - (float)cvmGet(epilines, i, 0)*pt2.x/(float)cvmGet(epilines, i, 1));

	cvSet(n_mask, cvScalar(255));
	cvLine(n_mask, pt1, pt2, cvScalar(0), 5, 8);

	if (mostrar_epi_lineas_poco_a_poco)
	{
	    int key;

	    cvLine(grey[1], pt1, pt2, cvScalar(250), 1, 8);
	    cvCircle (grey[0], pt0, 10, CV_RGB(200,200,200));
	    cvShowImage("h1", grey[0]);
	    cvShowImage("h2", grey[1]);
	    key = cvWaitKey(0);
	    key = key & (0x0FFFFF ^ 0x020000);
	    if (key == 'q' || key == 'Q')
		exit(-1);
	}

	//ROI para el resultado de la convolución. 
	//se busca alrededor de la línea epipolar
	if (pt1.y <= pt2.y)
	{
	    roiresult.y = pt1.y-2;
	    roiresult.height = pt2.y+2 - roiresult.y;
	}
	else 
	{
	    roiresult.y = pt2.y-2;
	    roiresult.height = pt1.y+2+1 - roiresult.y;
	}
	
	if (graficar)
	{
	    cvSetImageROI(graph, roigraph);
	    cvZero(graph);
	}

	//se eligen varios tamaños de parche
	unsigned int npatches = sizeof(match2)/sizeof(match2[0]);
	for (unsigned int m=0; m<npatches; m++)
	{
	    match = match2[m];
	    cvZero(result);
	    cvZero(graph1d);
	
	    //selección del parche a buscar
	    roipatch = cvRect(pt0.x-match.width/2, pt0.y-match.height/2, match.width, match.height);


	    // matching
	    // selección de la ventana de matching. parche en donde buscar
	    roimatch.width = roipatch.width + roiresult.width -1;
	    roimatch.height = roipatch.height + roiresult.height -1;
	    roimatch.x = roiresult.x - (int)roipatch.width/2;
	    roimatch.y = roiresult.y - (int)roipatch.height/2;
	    if (roimatch.y<0)
	    {
		break;
		//roimatch.height  -= -roimatch.y;
		//roiresult.height -= -roimatch.y;
		//roiresult.y += -roimatch.y;
		//roimatch.y += -roimatch.y;
		
	    }
	    //selección del parche a buscar (input)
	    cvSetImageROI(grey[0], roipatch);
	    // selección de las venetanas de búsqueda
	    cvSetImageROI(grey[1], roimatch);
	    cvSetImageROI(n_mask, roiresult);
	    cvSetImageROI(result, roiresult);
	
	    cvMatchTemplate(grey[1], grey[0], result, methods[method]);
	
	    // se sacan los valores de la linea epipolar de la correlación
	    cvSet(result, cvScalar(0), n_mask); 
	    cvReduce(result, graph1d, 0, CV_REDUCE_MAX);
	    //grafica(result, graph, graph1d, mask, cvmGet(epilines,i,0), cvmGet(epilines,i,1), cvmGet(epilines,i,2), m);
	    // se acumula el valor con las otras correlaciones
	    cvAdd(suma1d, graph1d, suma1d);
	
	    cvResetImageROI(grey[0]);
	    cvResetImageROI(grey[1]);
	    cvResetImageROI(n_mask);
	    cvResetImageROI(result);

	    if (graficar)
	    {
		grafica(graph, graph1d, m);
	    }
	}
	
	if (roimatch.y<0)
	{
	    printf("Point %d not valid. Out of range\n",i );
	    //el punto no es válido
	    continue;
	}

	//para encontrar el punto exacto, busca en máximo de la suma de las correspondencias
	// de los tres parches. Y entorno a ese máximo, busca el máximo del parche más pequeño.
	//búsqueda del máximo de los tres parches
	cvMinMaxLoc(suma1d, &min, &max, &minpos, &maxpos);
	// si se usa uno de los métodos de CV_TM_SQDIFF o CV_TM_SQDIFF_NORMED, se buscar el mínimo
	if (method<=1)
	{
	    maxpos = minpos; 
	    max = 1-min;
	}
	
	//evalua que el valor de correlación encontrado sea superior a un umbral
	if (max < MIN_CORR_VALUE*(float)npatches || maxpos.x < 10)
	{
	    printf("Point %d not valid (value %f en %d).\n", i, max/(float)npatches, maxpos.x);
	    //el punto no es válido
	    continue;
	}

	//ahora busca el máximo el parche más pequeño, en torno al punto indicado.
	// graph1d ya contiene la correlación del parche más pequeño
	CvMat subgraph1d;
	if (maxpos.x + 10 >= graph1d->width) 
	    maxpos.x = graph1d->width-11;
	if (maxpos.x < 10)
	    maxpos.x = 10;
	cvGetCols(graph1d, &subgraph1d, maxpos.x-10, maxpos.x+10+1);
	cvMinMaxLoc(&subgraph1d, &min, &max, &minpos, &maxpos2);
	if (method<=1) maxpos2 = minpos; 
	maxpos.x += maxpos2.x-10;

		    
	//la búsqueda se hizo con una ROI definida.
	//maxpos.x += roiresult.x;
	//maxpos.y += roiresult.y;
	
		
	//obtención del punto 3D correspondiente
	ur = (float)maxpos.x + roiresult.x;
	vr = pt1yfloat - (float)cvmGet(epilines, i, 0)*ur/(float)cvmGet(epilines, i, 1);
	
	GetStereoPoint(cvPoint2D32f(pt0.x, pt0.y), cvPoint2D32f(ur, vr), cal, model2D2->cal, XYZi);

	pmodel3D->setpoints(i, XYZi);
	model2D2->setpoints(i, ur, vr);

	cvSetReal1D(pmodel3D->mask, i, 1);

	printf("%3d: %7.1f  %7.1f  %7.1f -> %f\n", i,  
	       cvmGet(XYZi, 0,0), cvmGet(XYZi,1,0), cvmGet(XYZi,2,0), max/(float)npatches);

	if (graficar)
	{
	    cvScale(suma1d, suma1d, 1.0/(float)npatches);
	    cvLine(graph, cvPoint(maxpos.x, 0), cvPoint(maxpos.x, h-1), CV_RGB(100,100,100), 1);
	    grafica(graph, suma1d, 3);

	    printf("grafica %d:\n", i);

	    cvResetImageROI(graph);
	    cvShowImage("graph", graph);
	    
	    cvCopy(grey[0], grey2[0]);
	    cvCopy(grey[1], grey2[1]);

	    cvCircle(grey2[0], cvPoint(pt0.x, pt0.y), 10, CV_RGB(0,0,0), 1);
	    cvCircle(grey2[1], cvPoint((int)ur, (int)vr), 10, CV_RGB(0,0,0), 1);
	    cvShowImage("Camara Der", grey2[0]);
	    cvShowImage("Camara Izq", grey2[1]);
	    /*
	      if (0)
	      {
	      cvNamedWindow("graph2");
	      cvShowImage("graph2", img);
	      cvNamedWindow("graph3");
	      cvShowImage("graph3", img2);
	      }
	    */

	    while(1)
	    {
		int  key = cvWaitKey(0) ;
		key = key & (0x0FFFFF ^ 0x020000);

		if(key == 'n' || key == 'n'){
		    break;
		}
		else if(key == ' ' ){
		    break;
		}
		else if(key == 'q' || key=='Q' ){
		    graficar=0;
		    break;
		}
	    }
	}


    }
    
    cvDestroyWindow("graph");
    cvReleaseImage(&graph);

    if (graficar)
    {
	cvReleaseImage(&grey2[0]);
	cvReleaseImage(&grey2[1]);
    }

    return 1;

}

